/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2024-05-30     Dell       the first version
 */

#include "motor.h"

struct rt_device_pwm * motor_device;    // 舵机的pwm设备

int motor_purlse = MOTOR_BEGIN_PURLSE;
int dir = 1;


/**
 * 使舵机周期转动
 */
void motor_control(){
    motor_purlse = motor_purlse + dir * MOTOR_PURLSE_STEP;
    if(motor_purlse < MOTOR_MIN_PURLSE || motor_purlse > MOTOR_MAX_PURLSE){
        dir = - dir;
        motor_purlse = motor_purlse + dir * MOTOR_PURLSE_STEP;
    }
    RT_ASSERT(motor_purlse >= MOTOR_MIN_PURLSE && motor_purlse <= MOTOR_MAX_PURLSE);
    rt_err_t ret = rt_pwm_set(motor_device, MOTOR_PWM_CHANNEL, MOTOR_PERIOD, motor_purlse);
    if(ret != RT_EOK){
        rt_kprintf("rt_pwm_set failed\n", motor_purlse);
    }
}


/***
 * 初始化舵机设备
 */
void motor_init(){

    rt_device_t motor = rt_device_find(MOTOR_DEVICE_NAME);
    if(motor == RT_NULL){
        rt_kprintf("can not find %\n", MOTOR_DEVICE_NAME);
        return;
    }

    motor_device = (struct rt_device_pwm*) motor;
    rt_pwm_set(motor_device, MOTOR_PWM_CHANNEL, MOTOR_PERIOD, motor_purlse);    /* 设置PWM周期和脉冲宽度 */
    rt_pwm_enable(motor_device, MOTOR_PWM_CHANNEL);                             /* 使能设备 */

    // 初始化定时器
    rt_timer_t timer = rt_timer_create("motor_timer", motor_control, RT_NULL, 1000, RT_TIMER_FLAG_PERIODIC);
    if(timer != RT_NULL){
        rt_err_t res = rt_timer_start(timer);
        if(res == RT_EOK){
            rt_kprintf("motor start success\n");
            return;
        }
    }
//    rt_kprintf("motor start failed\n");
}
